// Fill out your copyright notice in the Description page of Project Settings.

#include "modules/map/hdmap/adapter/odr_parser/geometry_util/WorldArcIterator.h"

namespace apollo {

FWorldArcIterator::FWorldArcIterator()
{}

FWorldArcIterator::FWorldArcIterator(double curvature, double length, double sStart, double res, bool reverse)
{
	Reset(curvature, length, sStart, res, reverse);
}

bool FWorldArcIterator::HasNext() const
{
	return !_itEnd;
}

void FWorldArcIterator::Next()
{
	if (_reverse ? _s <= _sEnd : _s >= _sEnd)
	{
		_itEnd = true;
	}
	else
	{
		double s = _reverse ? std::max(_s-_res, _sEnd) : std::min(_s+_res, _sEnd);
		if (std::abs(_sEnd - s) < _res * 0.0001) s = _sEnd;
		_ds = s - _s;
		_s = s;
	}
}

void FWorldArcIterator::Reset(double curvature, double length, double sStart, double res, bool reverse)
{
	_curvature = curvature;
	_length = length;
	_res = res;
	_reverse = reverse;

	_radius = 1.0 / curvature;
	_center = glm::dvec2(0.0, _radius);
	_point = glm::dvec2(0.0, 0.0);
	_sStart = sStart;
	_sEnd = _reverse ? _sStart - length : _sStart + length;
	_s = _sStart;
	_ds = 0.0;
	_itEnd = false;
}

glm::dvec3 FWorldArcIterator::Sample()
{
	double rotAngle = _ds / glm::abs(_radius);
	double turn = _radius > 0 ? -1.0 : 1.0;
	if (_reverse) turn = -turn;

	glm::dvec2 v = _point - _center;
	v = glm::dvec2(
		v.x * std::cos(rotAngle) + turn * v.y * std::sin(rotAngle),
		-1.0 * turn * v.x * std::sin(rotAngle) + v.y * std::cos(rotAngle));
	_point = v + _center;

	return glm::dvec3(_point, 1.0);
}

glm::dvec3 FWorldArcIterator::SampleDrv()
{
	double rotAngle = _s / glm::abs(_radius);
	double turn = _radius > 0 ? -1.0 : 1.0;
	if (_reverse) turn = -turn;

	glm::dvec2 ret(std::cos(rotAngle), -turn * std::sin(rotAngle));

	return glm::dvec3(ret, 0);
}

double FWorldArcIterator::S() const
{
	return _s;
}

} // namespace